#include "tf2_ros/transform_listener.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/PointStamped.h"

#include "upros_message/ArmPosition.h"
#include "std_srvs/Empty.h"
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <apriltag_ros/AprilTagDetectionArray.h>

void sleep(double second)
{
    ros::Duration(second).sleep();
}

double current_x;
double current_z;

// PID控制参数
const double Kp = 3;                    // 比例系数
const double target_x_tolerance = 0.04; // X轴位置容忍误差

const double z_target_distance = 0.20;
const double target_z_tolerance = 0.02;

bool aim_to_tag = false;

int tag_id = 1;

void tagCallback(const apriltag_ros::AprilTagDetectionArray::ConstPtr &msg)
{
    bool target_found = false;

    for (const auto &detection : msg->detections)
    {
        if (detection.id[0] == tag_id)
        {
            current_x = detection.pose.pose.pose.position.x;
            current_z = detection.pose.pose.pose.position.z;
        }
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "mgrab_test");
    ros::AsyncSpinner spinner(1);
    spinner.start();
    ros::NodeHandle nh;
    // 使用到的 service
    ros::ServiceClient arm_move_close_client = nh.serviceClient<upros_message::ArmPosition>("/upros_arm_control/arm_pos_service_close");
    ros::ServiceClient arm_zero_client = nh.serviceClient<std_srvs::Empty>("/upros_arm_control/zero_service");

    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    ros::Subscriber tag_sub_ = nh.subscribe("tag_detections", 1, tagCallback);

    ros::Rate loop_rate(10);

    geometry_msgs::Twist vel_msg;

    // 首先，对齐到下方的ApriTag码
    while (aim_to_tag == false)
    {
        if ((fabs(current_x) < target_x_tolerance) && (fabs(current_z - z_target_distance) < target_z_tolerance))
        {
            ROS_INFO("AIM!!!!!!");
            aim_to_tag = true;
            vel_msg.angular.z = 0;
            vel_msg.linear.x = 0;
        }
        else if (fabs(current_x) > target_x_tolerance)
        {
            vel_msg.angular.z = Kp * (-current_x);
        }
        else if (fabs(current_z - z_target_distance) > target_z_tolerance)
        {
            vel_msg.linear.x = Kp * 0.3 * (current_z - z_target_distance);
        }
        pub.publish(vel_msg);
        loop_rate.sleep();
        ros::spinOnce();
    }

    // 第二步，前进一段距离,马桶刷进入马桶区域
    vel_msg.linear.x = 0.1;
    int count = 0;
    while (ros::ok() && count < 20)
    {
        pub.publish(vel_msg);
        ros::spinOnce();
        loop_rate.sleep();
        count++;
    }
    vel_msg.linear.x = 0.0;
    pub.publish(vel_msg);

    // 第三步，向下触发光电
    upros_message::ArmPosition move_srv;
    move_srv.request.x = 0.0;
    move_srv.request.y = 300.0;
    move_srv.request.z = 120.0;
    arm_move_close_client.call(move_srv);
    sleep(3.0);

    // 第四步，向上抬起
    move_srv.request.x = 0;
    move_srv.request.y = 250;
    move_srv.request.z = 200;
    arm_move_close_client.call(move_srv);
    sleep(5.0);

    // 第五步，后退30cm，脱离马桶范围
    vel_msg.linear.x = -0.1;
    count = 0;
    while (ros::ok() && count < 40)
    {
        pub.publish(vel_msg);
        ros::spinOnce();
        loop_rate.sleep();
        count++;
    }
    vel_msg.linear.x = 0.0;
    pub.publish(vel_msg);

    ros::shutdown();

    return 0;
}
